# -*- coding: utf-8 -*-
# mavlink parser for a buffer support for stream input and count unparserable block
# Mavlink packet structure- see: http://qgroundcontrol.org/mavlink/start
from ctypes import *
# checsumDll=CDLL('mavlinkChecksum.dll')
from mavcrc import *

# def crc_calculate(msgStream):
#     '''
#         calculate the checksum
#     '''
#     length = c_uint16(len(msgStream))
#     string = c_char_p(msgStream)
#     crc_calc = checsumDll.pub_crc_calculate_final
#     # crc_calc = checsumDll.pub_crc_calculate
#     crc_calc.restype= c_uint16
#     # print 'heared'
#     result =crc_calc(string,length)
#     # print 'hear'
#     cra    = result%256
#     crb    = result/256
#     crcresult = chr(cra)+chr(crb)
    
#     return crcresult

def crc_calculate(buf,msgid):
    buf = buf + chr(MAVLINK_MESSAGE_CRCS[msgid])
    crccalcer = x25crc(buf)
    result=crccalcer.crc
    cra    = result%256
    crb    = result/256
    crcresult = chr(cra)+chr(crb)
    return crcresult



class mavlinkParser(object):
    def __init__(self,BUFFERMAXSIZE=None):
        self.buffer = ''
        if BUFFERMAXSIZE is None:
            BUFFERMAXSIZE = 260
        self.BUFFERMAXSIZE = BUFFERMAXSIZE

    def push(self,pushStr):
        '''
            will push a str into the buffer, and automatically call parse function 
        '''
        self.buffer = self.buffer+pushStr
        if(len(self.buffer)>self.BUFFERMAXSIZE):
            return self.parseMsg()
        else:
            return None,0
        
    def clear(self):
        '''
            clear buffer
        '''
        self.buffer =''
    def cut(self,upto):
        '''
            cut the buffer up to upto
        '''
        self.buffer=self.buffer[(upto+1):len(self.buffer)]
        
    def parseMsg(self):
        '''
            parse msg based on the buffer
            will parse all of the possible msg

            consider a msg contains 0xfe?
            TODO return the trimmed data
            typedef struct __mavlink_message {
                uint16_t checksum; ///< sent at end of packet
                uint8_t magic;   ///< protocol magic marker
                uint8_t len;     ///< Length of payload
                uint8_t seq;     ///< Sequence of packet
                uint8_t sysid;   ///< ID of message sender system/aircraft
                uint8_t compid;  ///< ID of the message sender component
                uint8_t msgid;   ///< ID of message in payload
                uint64_t payload64[(MAVLINK_MAX_PAYLOAD_LEN+MAVLINK_NUM_CHECKSUM_BYTES+7)/8];
            }mavlink_message_t;
            
        '''
        bufferLastLen = len(self.buffer)
        validMsgs = []
        invalidMsgs = []
        headFound = False
        indexLack = False
        lastHeadIndex = 0
        bufferLen = len(self.buffer)
        key = -1
        lastTailIndex = -1 
        while True:
            key = key + 1 
            if(key>=bufferLen):
                break
            head = self.buffer[key]
            if head == chr(0xfe):
                msg={}
                headFound = True
                try:
                    msg['startIndex'] = key
                    msg['len']=ord(self.buffer[key+1])
                    if key+8+msg['len']>len(self.buffer):
                        raise IndexError()
                    msg['seq']=ord(self.buffer[key+2])
                    msg['sysid']=ord(self.buffer[key+3])
                    msg['compid']=ord(self.buffer[key+4])
                    msg['msgid']=ord(self.buffer[key+5])
                    msg['payload']=self.buffer[(key+6):(key+6+msg['len'])]
                    crcStr= self.buffer[(key+1):(key+6+msg['len'])]
                    crc   =  self.buffer[(key+6+msg['len']):(key+8+msg['len'])]
                    msg['crc'] = crc
                    crcExpect = crc_calculate(crcStr,msg['msgid'])
                    # print ":".join("{:02x}".format(ord(c)) for c in crcExpect)
                    if crcExpect == crc:
                        validMsgs.append(msg)
                        key = key+ 7 + msg['len']
                        lastTailIndex = key
                        # print 'valid msg'
                    else:
                        invalidMsgs.append(msg)
                        lastTailIndex = key+ 7 + msg['len']
                        # print "invalid msg"
                except IndexError:
                    indexLack = True
                    break          
        if headFound == False or indexLack == False : 
            self.clear()
        else:
            self.cut(lastTailIndex)

        validMsgLen = 0
        for validMsg in validMsgs:
            validMsgLen = validMsgLen + validMsg['len']+8
        abandonedCount = bufferLastLen - len(self.buffer) - validMsgLen
        return validMsgs,abandonedCount

if __name__ =="__main__":
    # m  =  mavlinkParser()
    # m.push('FE1C20630021F8FDD3008085B50D806E5A43000000000000000098CED300F8FDD3002852FE1C20630021F8FDD3008085B50D806E5A43000000000000000098CED300F8FDD3002852feFE1C20630021F8FDD3008085B50D806E5A43000000000000000098CED300F8FDD3002852'.decode('hex'))
    # print len(m.buffer)
    # validMsgs,abandonedCount = m.parseMsg()
    # print validMsgs,abandonedCount,len(m.buffer)

    # print ":".join("{:02x}".format(ord(c)) for c in m.buffer)
    #'C01E'
    # k = '1C00630021F8FDD3008085B50D806E5A43000000000000000098CED300F8FDD300'.decode('hex') 
    # import mavcrc
    #crcCheck = mavcrc.x25crc(k)
    #crcCheck.accumulate_str()

    a = '282E0101B801294601FF674714998EC7434A6101008CFDFFFF40AFB33F5834413EFCFF1200000050461B000000'.decode('hex')
    crc1 = crc_calculate(a,184)
    # crc2 = crc_calculate2(a,33)    
    print hex(ord(crc1[0])),hex(ord(crc1[1]))